#include <unistd.h> // alarm
#include <signal.h> // alarm

#include "WallFollower.h"
#include "std_msgs/Float64.h"

#define WALL_FOLLOW_SPEED 0.35 // default speed while following wall (meter/second)
#define FIXING_DISTANCE 0.4 // vertical distance that the robot can drive to correct angle while following wall (meter)
#define TOLERANCE 0.01 // error allowed while deciding if robot is too close or far from the wall
#define CORNER_DISTANCE 0.7 // front-distance that indicates a corner
#define OBSTACLE_DISTANCE 0.35 // front-distance for a sudden obstacle
#define EMERGENCY_DISTANCE 0.18 // front-distance for emergency stop
#define ESCAPE_CORNER_ANGLE 1.7 // turn angle when reaches front wall of a corner
#define TILT_TOLERANCE 0.007 // threshold of difference between front-side and side samples when robot is aligned
#define FRONT_SIDE_DIST 0.24 // distance between front and side sensors
#define MIN_ANGLE 0.1 // minimal alignment angle (radians) allowed
#define TIME_FOR_EVENT 25000 // time to enable event handling (micro-seconds)

#define TOPIC_NAME "/Sensors_Module/Front_Laser_Range/WallFollower" // full-name of topic used to receive sensors' data
#define TOPIC_SUFFIX "WallFollower" // suffix used to identify the color tracker while receiving sensors' data


bool keep_running = true;
/*
 * Signal handler is called when countdown is over
 */
void stopRunning(int signum) {

	keep_running = false;
}

WallFollower::WallFollower(Driver* driver, double speed, double wallDistance) {

	m_driver = driver;
	m_speed = speed;
	m_goalWallDist = wallDistance;
	m_sideDist = 0;

	m_loop_rate = new ros::Rate(1);
	m_fast_loop_rate = new ros::Rate(100);

	// subscribe to sensors module
	int queue_depth = 1000;

	m_sensors_publisher = m_node_handler.advertise<std_msgs::String>(
			"/Sensors_Module/Subscribers_Channel", 1000);

	// subscribe to front laser
	sub_sensors_fl = m_node_handler.subscribe("/Sensors_Module/Front_Laser/WallFollower",
			queue_depth, &WallFollower::frontLaserHandler, this);

	// subscribe to right part of front laser
	sub_sensors_frl = m_node_handler.subscribe("/Sensors_Module/FrontRight_Laser/WallFollower", queue_depth,
			&WallFollower::frontRightLaserHandler, this);

	// subscribe to left part of front laser
//	sub_sensors_frl = m_node_handler.subscribe(
//			"/Sensors_Module/FrontRight_Laser/WallFollower", queue_depth,
//			&WallFollower::IMPLEMENT_CALLBACK, this);

	// subscribe to right laser
	sub_sensors_rl = m_node_handler.subscribe("/Sensors_Module/Right_Laser/WallFollower",
			queue_depth, &WallFollower::rightLaserHandler, this);
	
	// subscribe to right ultra-sonic
	sub_sensors_right_us = m_node_handler.subscribe("/Sensors_Module/Right_US/WallFollower",
			queue_depth, &WallFollower::rightUSHandler, this);

	// subscribe to left laser
	sub_sensors_ll = m_node_handler.subscribe("/Sensors_Module/Left_Laser/WallFollower",
			queue_depth, &WallFollower::leftLaserHandler, this);
	
	// subscribe to left ultra-sonic
	sub_sensors_left_us = m_node_handler.subscribe("/Sensors_Module/Left_US/WallFollower",
			queue_depth, &WallFollower::leftUSHandler, this);
	
	// subscribe to back laser
	sub_sensors_bl = m_node_handler.subscribe("/Sensors_Module/Back_Laser/WallFollower",
			queue_depth, &WallFollower::backLaserHandler, this);

	// subscribe to back ultra-sonic
	sub_sensors_back_us = m_node_handler.subscribe("/Sensors_Module/Back_US/WallFollower",
			queue_depth, &WallFollower::backUSHandler, this);

	// force connection establishment
	m_loop_rate->sleep();
}

WallFollower::~WallFollower() {

	desubscribe();
}

void WallFollower::desubscribe() {

	// deactivate necessary sensors
	subscribeToSensor(sensors_names::FRONT_LASER, 		false, CORNER_DISTANCE, MAX_SENSOR_RANGE);
	subscribeToSensor(sensors_names::FRONT_RIGHT_LASER, false, m_goalWallDist-TOLERANCE, m_goalWallDist+TOLERANCE);
	subscribeToSensor(sensors_names::RIGHT_LASER, 		false, MAX_SENSOR_RANGE, 0);
}

void WallFollower::subscribeToSensor(sensors_names::ordinal sensor,
		bool shouldSubscribe, double min_distance, double max_distance) {

	// compose message
	char buffer[128];
	snprintf(buffer, sizeof(buffer), "%s%s%d%s%d%s%f%s%f", TOPIC_SUFFIX,
			DELIMITER, sensor, DELIMITER,
			int(shouldSubscribe), DELIMITER, min_distance, DELIMITER, max_distance);

	m_message_to_publish.data = buffer;

	// publish the message to sensors
	m_sensors_publisher.publish(m_message_to_publish);
}

void WallFollower::frontLaserHandler(const std_msgs::String::ConstPtr& msg) {

	// check validity
	if (NULL == m_driver) {
		ROS_ERROR("DRIVER IS NULL");
		return;
	}

	Event event(sensors_names::FRONT_LASER, atof(msg->data.c_str()));
	event_queue.push(event);
}

void WallFollower::frontRightLaserHandler(
		const std_msgs::String::ConstPtr& msg) {

	// check validity
	if (NULL == m_driver) {
		ROS_ERROR("DRIVER IS NULL");
		return;
	}

	Event front_right_lsr_dummy(sensors_names::FRONT_RIGHT_LASER,0.0);

	int index = event_queue.indexOf(front_right_lsr_dummy);
	if (ERROR_INDEX != index)
	{
		return;
	}

	Event event(sensors_names::FRONT_RIGHT_LASER, atof(msg->data.c_str()));
	event_queue.push(event);
}

// used for example of subscription, could be implemented for future use
void WallFollower::backLaserHandler(
		const std_msgs::String::ConstPtr& msg) {

}

// used for example of subscription, could be implemented for future use
void WallFollower::backUSHandler(const std_msgs::String::ConstPtr& msg) {

}

// used for example of subscription, could be implemented for future use
void WallFollower::rightUSHandler(
		const std_msgs::String::ConstPtr& msg) {

}

void WallFollower::rightLaserHandler(const std_msgs::String::ConstPtr& msg) {

	// check validity
	if (NULL == m_driver) {
		ROS_ERROR("DRIVER IS NULL");
		return;
	}

	ROS_ERROR("IN WallFollower::rightLaserHandler");
	// update last sample
	setSideDistance(atof(msg->data.c_str()));
}

// used for example of subscription, could be implemented for future use
void WallFollower::leftUSHandler(const std_msgs::String::ConstPtr& msg) {

}

// used for example of subscription, could be implemented for future use
void WallFollower::leftLaserHandler(
		const std_msgs::String::ConstPtr& msg) {

}

void WallFollower::follow() {

	// keep driving and maintain direction
	maintainDirection();
}

void WallFollower::follow(unsigned int time_to_run) {

	if (time_to_run <= 0) {
		// block function
		follow();
	}

	// register to alarm signal
	struct sigaction sact;
	sigemptyset(&sact.sa_mask);
	sact.sa_flags = 0;
	sact.sa_handler = stopRunning;
	sigaction(SIGALRM, &sact, NULL);

	keep_running = true;
	alarm(time_to_run);

	// keep driving and maintain direction
	maintainDirection();

	// Stop at exit
	m_driver->stop();

	desubscribe();
}

void WallFollower::maintainDirection() {

	// set maximal numbers of events to handle
	event_queue.setMaxSize(MSG_QUEUE_SIZE);

	// activate necessary sensors
	subscribeToSensor(sensors_names::FRONT_LASER, 		true, CORNER_DISTANCE, MAX_SENSOR_RANGE);
	subscribeToSensor(sensors_names::FRONT_RIGHT_LASER, true, m_goalWallDist-TOLERANCE, m_goalWallDist+TOLERANCE);
	subscribeToSensor(sensors_names::RIGHT_LASER, 		true, MAX_SENSOR_RANGE, 0);

	// set for how long should handle each event
	long int timeForEvent = TIME_FOR_EVENT; // calculation of time: 0.25 = 25e-2 = (25e4)*1e-6

	// read events from queue and handle them
	while (ros::ok() && keep_running) {

		ros::spinOnce();

		while (!event_queue.empty()) {

			// handle event at queue's head
			Event currEvent = event_queue.pop();

			switch (currEvent.m_sensor) {

			// handle obstacle ahead
			case  sensors_names::FRONT_LASER: {

				// handle obstacles
				if (currEvent.m_data <= OBSTACLE_DISTANCE) {

					// if too close, stop
					if (currEvent.m_data <= EMERGENCY_DISTANCE) {
						m_driver->stop();
						break;
					}

					// not too close to obstacle, stop and try bypassing
					m_driver->stop();
					usleep(50000);
					m_driver->turn(ESCAPE_CORNER_ANGLE);
					usleep(50000);

				} else {
					// obstacle is a corner- turn left
					m_driver->turn(ESCAPE_CORNER_ANGLE);
				}

				break;
			}

			// robot is either too close or too far from the wall
			case sensors_names::FRONT_RIGHT_LASER: {

				ROS_INFO("FRONT_RIGHT_LASER--currEvent.m_data: %.3f",currEvent.m_data);

				// fix robot's distance from wall according to sampled data
				fixDistance(currEvent);
				break;
			}

			case sensors_names::RIGHT_LASER: {
				// event is handled by storing newest sample
				break;
			}


				default: {
					ROS_ERROR("WallFollower switch default unpredictable call! value= %d", currEvent.m_sensor);
				}
			}	//Switch end

			// set process to sleep in order to enable influence of event handling
			usleep(timeForEvent);
		}

		// no event, keep driving forward
		m_driver->move(m_speed, 0);
	}
}

void WallFollower::fixDistance(const Event& currEvent) {

	// calculate angle to align robot
	double align_angle = align(currEvent);

	// calculate angle to fix its distance
	// (order of operands must be as written, otherwise turn angle's sign is wrong)
	double fix_angle = turnAngle(m_goalWallDist - currEvent.m_data);

	// scale-up angles
	double scale = 100000/TIME_FOR_EVENT;

	ROS_INFO("fix angle: %f, rads: %f", fix_angle*(180/(2*HALF_PI)), fix_angle);

	// choose turning angle

	if ((fix_angle*align_angle) <= 0) {
		m_driver->turn(scale*(fix_angle + align_angle));
		return;
	}

	if (fix_angle < align_angle) {
		m_driver->turn(scale*align_angle);
		return;
	}

	m_driver->turn(scale*fix_angle);
}

double WallFollower::align(const Event& event) {

	// read last sample from side sensor
	double sideDist = getSideDistance();

	// calculate angle to fix tilt (negative angle means turn right)
	double fixAngle = -atan((event.m_data-sideDist) / FRONT_SIDE_DIST);

	ROS_INFO("alignment angle: %f, rads: %f. Side Distance = %f", fixAngle*(180/(2*HALF_PI)), fixAngle,sideDist);

	// return result
	return fixAngle;
}

double WallFollower::turnAngle(double horizontalDist) {

	// avoid zero division
	if (0 == horizontalDist) {
		return 0; // do not turn
	}

	ROS_INFO("horizontal distance: %f ; turn angle (rads): %f", horizontalDist, atan(horizontalDist / (FIXING_DISTANCE*(m_speed+1))));

	// calculate angle
	return atan(horizontalDist / (FIXING_DISTANCE*(m_speed+1)));
}

double WallFollower::getSideDistance() {

	double sideDist = 0;
	std::unique_lock<std::mutex>
	mlock(m_mutex);
	sideDist = m_sideDist;
	mlock.unlock();
	return sideDist;
}

void WallFollower::setSideDistance(double newDist) {

	std::unique_lock<std::mutex>
	mlock(m_mutex);
	m_sideDist = newDist;
	ROS_ERROR("new side distance: %f", m_sideDist);
	mlock.unlock();
}
